import numpy
import numpy as np



def generate_forbidden_list(reservation):
    forbidden_set = set()  # 使用集合避免重复元素
    for row in reservation:
        occupied_positions = np.where(row == 1)[0]  # 获取所有被占用的位置索引
        # print(occupied_positions)
        differences = np.abs(occupied_positions[:, None] - occupied_positions)  # 计算位置间的距离
        # print(differences)
        # 排除自身与自身的差值（即0），并将结果展平为一维数组
        non_zero_differences = differences[differences > 0].flatten()
        forbidden_set.update(non_zero_differences)

    # 将集合转换回列表并返回
    return list(forbidden_set)





def generate_collision_vector(forbidden_list,length):
    collision_vector = np.zeros(length-1,dtype=int)
    for i in forbidden_list:
        collision_vector[i-1] = 1
    return np.flip(collision_vector)





def right_shifted_and_or(collision_vector,offset,original_vector):
    length = len(collision_vector)
    if offset<=length:
        new_collision_vector = np.concatenate((np.zeros(offset,dtype=int),collision_vector[:length-offset]))
        new_collision_vector=np.bitwise_or(new_collision_vector,original_vector)
    else:
        new_collision_vector = original_vector
    return new_collision_vector



def check_possible_offset(collision_vector, hyperoffset):
    length = len(collision_vector)
    position = np.array(np.where(collision_vector == 0))
    position = length - position
    if hyperoffset:
        position = numpy.append(position, length + 1)
    else:
        position = position[0]

    return position


def generate_adjacency_list(collision_vector, max_iterations=10, hyperoffset = False):
    """
    根据初始collision_vector生成网络的邻接表
    返回：向量列表、邻接表
    """
    vector_list = []  # 存储所有生成的向量
    vector_map = {}   # 用于快速索引向量的映射
    adjacency_list = {}  # 用于存储邻接表

    # 添加初始向量
    vector_list.append(collision_vector)
    vector_map[tuple(collision_vector)] = 0  # 初始节点索引为0
    adjacency_list[0] = []  # 初始化邻接表的第一个节点

    # 生成网络
    for iteration in range(max_iterations):
        new_vectors = []  # 存储新生成的向量

        for idx, vector in enumerate(vector_list):
            # 获取当前向量的所有可能偏移
            offsets = check_possible_offset(vector,hyperoffset)

            for offset in offsets:
                new_vector = right_shifted_and_or(vector, offset,collision_vector)
                vector_tuple = tuple(new_vector)

                # 检查新向量是否已经存在
                if vector_tuple not in vector_map:
                    # 如果是新向量，加入列表和映射
                    vector_list.append(new_vector)
                    new_idx = len(vector_list) - 1
                    vector_map[vector_tuple] = new_idx
                    adjacency_list[new_idx] = []  # 初始化邻接表新节点
                    new_vectors.append(new_vector)
                else:
                    new_idx = vector_map[vector_tuple]

                # 更新邻接表
                if (new_idx, offset) not in adjacency_list[idx]:
                    adjacency_list[idx].append((new_idx, offset))

        # 如果没有新的向量生成，退出迭代
        if not new_vectors:
            break

    return vector_list, adjacency_list

def adjacency_list_to_matrix(adjacency_list, num_nodes):
    """
    根据邻接表生成邻接矩阵，保留每对节点之间最小的边权
    """
    adjacency_matrix = np.full((num_nodes, num_nodes), np.inf)  # 使用inf表示未连接的节点

    for src, edges in adjacency_list.items():
        for dst, weight in edges:
            # 如果当前权值更小，则更新矩阵
            if weight < adjacency_matrix[src][dst]:
                adjacency_matrix[src][dst] = weight


    return adjacency_matrix

def reconstruct_cycle(start, end, next_node):
    """通过next_node矩阵重建从start到end的路径"""
    if start == end:
        return [start,end]
    path = [start]
    while start != end:
        start = next_node[start][end]
        if start == -1:  # 防止错误路径
            return None
        path.append(start)
    return path


def floyd_warshall(adjacency_matrix):
    """Floyd-Warshall算法计算最短路径"""
    num_nodes = len(adjacency_matrix)
    dist = adjacency_matrix.copy()  # 初始化距离矩阵
    next_node = np.full_like(adjacency_matrix, -1, dtype=int)  # 用于追踪路径

    # 初始化next_node矩阵
    for i in range(num_nodes):
        for j in range(num_nodes):
            if dist[i][j] != np.inf and i != j:
                next_node[i][j] = j

    # 进行Floyd-Warshall更新
    for k in range(num_nodes):
        for i in range(num_nodes):
            for j in range(num_nodes):
                if dist[i][j] > dist[i][k] + dist[k][j]:
                    dist[i][j] = dist[i][k] + dist[k][j]
                    next_node[i][j] = next_node[i][k]

    return dist, next_node


def find_minimum_average_cycle(adjacency_matrix):
    """寻找最小平均权重环并返回环路节点序号"""
    num_nodes = len(adjacency_matrix)

    # 使用Floyd-Warshall算法计算最短路径
    dist, next_node = floyd_warshall(adjacency_matrix)

    min_avg_weight = np.inf
    best_cycle_path = None

    # 遍历所有节点作为起点，查找从该节点出发返回自己的环
    for i in range(num_nodes):
        for j in range(num_nodes):
            if dist[i][j] != np.inf and dist[j][i] != np.inf:
                # 检查从i到j再回到i的环路
                total_weight = dist[i][j] + dist[j][i]  # 环的总权重
                path_to_j = reconstruct_cycle(i, j, next_node)  # 从i到j的路径
                path_to_i = reconstruct_cycle(j, i, next_node)  # 从j到i的路径

                if path_to_j and path_to_i:
                    cycle_path = path_to_j + path_to_i[1:]  # 合并路径，去重回点
                    cycle_length = len(cycle_path) - 1  # 环路总长度
                    avg_weight = total_weight / cycle_length  # 平均权重

                    if avg_weight < min_avg_weight:
                        min_avg_weight = avg_weight
                        best_cycle_path = cycle_path
    if best_cycle_path[0] != 0:
        best_cycle_path = reconstruct_cycle(0,best_cycle_path[0],next_node) + best_cycle_path[1:]
    return min_avg_weight, best_cycle_path


def bezier_midpoint(p0, p2, rad):
    """
    Calculate the midpoint of a quadratic Bezier curve defined by start point p0,
    end point p2, and curvature rad.

    :param p0: Start point coordinates as a tuple (x1, y1)
    :param p2: End point coordinates as a tuple (x2, y2)
    :param rad: Curvature factor for the Bezier curve
    :return: Midpoint coordinates as a tuple (xm, ym)
    """
    x1, y1 = p0
    x2, y2 = p2

    # Calculate the middle point M between p0 and p2
    mx = (x1 + x2) / 2
    my = (y1 + y2) / 2

    # Calculate direction vector from p0 to p2
    dx = x2 - x1
    dy = y2 - y1

    # Calculate perpendicular direction vector
    perp_dx = -dy
    perp_dy = dx

    # Normalize perpendicular vector
    length = np.sqrt(perp_dx ** 2 + perp_dy ** 2)
    perp_dx /= length
    perp_dy /= length

    # Adjust the perpendicular vector according to the curvature
    control_x = mx + rad * perp_dx
    control_y = my + rad * perp_dy

    # Calculate the midpoint of the Bezier curve
    xm = (x1 + 2 * control_x + x2) / 4
    ym = (y1 + 2 * control_y + y2) / 4

    return (xm, ym)


if __name__ == "__main__":
    reservation = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 1],
                            [0, 1, 1, 0, 0, 0, 0, 1, 0],
                            [0, 0, 0, 1, 0, 0, 0, 0, 0],
                            [0, 0, 0, 0, 1, 1, 0, 0, 0],
                            [0, 0, 0, 0, 0, 0, 1, 1, 0]])
    forbidden_list = generate_forbidden_list(reservation)
    collision_vector = generate_collision_vector(forbidden_list, reservation.shape[1])
    # 调用函数并打印结果
    print(forbidden_list, collision_vector)
    print(right_shifted_and_or(collision_vector, 6, collision_vector))
    # print(check_possible_offset(collision_vector))
    # 调用函数生成邻接表
    vector_list, adjacency_list = generate_adjacency_list(collision_vector, max_iterations=10)

    # 根据邻接表生成邻接矩阵
    adjacency_matrix = adjacency_list_to_matrix(adjacency_list, num_nodes=len(vector_list))

    # 打印生成的向量列表
    print("Generated Vector List:")
    for i, vec in enumerate(vector_list):
        print(f"Vector {i}: {vec}")

    # 打印邻接表
    print("\nAdjacency List:")
    for src, edges in adjacency_list.items():
        print(f"Node {src}: {edges}")

    # 打印邻接矩阵
    print("\nAdjacency Matrix:")
    print(adjacency_matrix)

    # 调用函数计算最小平均权重环
    min_avg_weight, best_cycle = find_minimum_average_cycle(adjacency_matrix)

    # 打印结果
    print(f"最小平均权重环: {min_avg_weight}")
    print(f"最优环: {best_cycle}")
    for i in best_cycle:
        print(vector_list[i])
